//-----------------------------------------------------------------------------
//         Headers
//------------------------------------------------------------------------------

#include "main.h"

#include "adc.h"
#include "usb.h"
#include "wait.h"
#include "s_spi.h"
#include "gyro.h"
#include "accel.h"
#include "compass.h"
#include "pid.h"
//#include <memories/flash/flashd_efc.c>

//------------------------------------------------------------------------------
//      Internal variables
//------------------------------------------------------------------------------

#define USB_READ_PACKET_SIZE 6

short Start = 0;
float Throttle = 0.0;

Compass compass;
Compass compass_calibration;

unsigned char usbBufferRead[DATABUFFERSIZE];

char usbReadRunning = 0;

static unsigned short ProgramState = STATE_ACCEL_X;
static unsigned short WorkState = STATE_RESUME;

// TODO: Delete this
float IMU_ADC_ACCELX_SCALE = IMU_ADC_ACCEL_SCALE_1_5g;
float IMU_ADC_ACCELY_SCALE = IMU_ADC_ACCEL_SCALE_1_5g;
float IMU_ADC_ACCELZ_SCALE = IMU_ADC_ACCEL_SCALE_1_5g;

unsigned short IMU_ADC_ROLL_DOT_ZERO = 28800;
unsigned short IMU_ADC_PITCH_DOT_ZERO = 28800;
unsigned short IMU_ADC_YAW_DOT_ZERO = 28800;

//------------------------------------------------------------------------------
//         Internal functions
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
/// Interrupt handler for TC0 interrupt. Increment program state
//------------------------------------------------------------------------------
void ISR_Tc0(void)
{
    // Clear status bit to acknowledge interrupt
    AT91C_BASE_TC0->TC_SR;
    
    // Change to next program state
    ProgramState = (ProgramState % STATE_COUNT == STATE_IDLE) ? STATE_CALCULATION : (ProgramState + 1);
    WorkState = STATE_RESUME;
}

//------------------------------------------------------------------------------
/// Callback invoked when data has been received on the USB.
//------------------------------------------------------------------------------
void UsbDataReceived(unsigned int unused,
                            unsigned char status,
                            unsigned int received,
                            unsigned int remaining)
{
    // Check that data has been received successfully
    if (status == USBD_STATUS_SUCCESS)
    {
      unsigned short usbReadPointer = 0;
      
      while (received - usbReadPointer >= USB_READ_PACKET_SIZE)
      {
        if (usbBufferRead[usbReadPointer] == 0xFF)
        {
          unsigned char Message = usbBufferRead[usbReadPointer+1];
          
          //if (Message <= 16)
          //{
            long Buf = (usbBufferRead[usbReadPointer+2] << 24) | (usbBufferRead[usbReadPointer+3] << 16) | (usbBufferRead[usbReadPointer+4] << 8) | (usbBufferRead[usbReadPointer+5]);
            float Param = *((float *)&Buf);
          
            //unsigned short Header2 = 0x10FF;
          
            switch (Message)
            {
              case 10:
              {
                compass_calibration.compass_zero[0] = Param;
                
                break;
              }
              case 11:
              {
                compass_calibration.compass_zero[1] = Param;
                
                break;
              }
              case 12:
              {
                compass_calibration.compass_zero[2] = Param;
                
                break;
              }
              case 13:
              {
                compass_calibration.compass_scale[0] = 1.0f / Param;
                
                break;
              }
              case 14:
              {
                compass_calibration.compass_scale[1] = 1.0f / Param;
                
                break;
              }
              case 15:
              {
                compass_calibration.compass_scale[2] = 1.0f / Param;
                
                break;
              }
              case 16:
              {
                compass_calibrate(&compass_calibration);
                
                LED_Toggle(0);
                
                break;
              }
              case 17:
              {
                PIO_Set(&pinGyroReset);
                Delay(1000);
                PIO_Clear(&pinGyroReset);
                
                break;
              }
              case 18:
              {
                ahrs_euler_zero[0] = ahrs_euler[0];
                ahrs_euler_zero[1] = ahrs_euler[1];
                ahrs_euler_zero[2] = ahrs_euler[2];
                
                break;
              }
              case 19:
              {
                if (Param == 1.0f)
                {
                  Start = 1;
                }
                else
                {
                  Start = 0;
                }
                
                break;
              }
              case 20:
              {
                Throttle = Param;
                
                break;
              }
              //while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)&Param, sizeof(Param), 0, 0));
              //while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)&Header2, sizeof(Header2), 0, 0));
              //Wait(4);           
            }
          /*}
          else
          {
            
              //unsigned short Param = (usbBufferRead[usbReadPointer+2] << 8) | usbBufferRead[usbReadPointer+3];              
            
          }*/
          usbReadPointer += USB_READ_PACKET_SIZE - 1;          
        }        
        usbReadPointer++;
        //while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write(usbBufferRead, received, /*(TransferCallback) UsbDataTransfered*/0, 0));
      }
    }
    usbReadRunning = 0;
}
  
//------------------------------------------------------------------------------
//          Main
//------------------------------------------------------------------------------

int main()
{
  //int a = 100;
    LED_Configure(0);
    LED_Set(0);
    
    TRACE_CONFIGURE(DBGU_STANDARD, 115200, BOARD_MCK);
    
    PIO_Configure(&pinMotor1, 1);
    
    ConfigurePit();
    ConfigureTc();    
    
    PIO_Configure(pinsSPI, PIO_LISTSIZE(pinsSPI));
    
    PIO_Configure(&pinMagReset, 1);
    PIO_Configure(&pinGyroReset, 1);
    
    PIO_Configure(&pinADCDataReady, 1);
    
    PIO_Configure(&pinAccelGS1, 1);
    PIO_Configure(&pinAccelGS2, 1);
    
    // Set 1.5g sensitivity on accel
    PIO_Clear(&pinAccelGS1);
    PIO_Clear(&pinAccelGS2);
    
    PIO_Configure(&pinMotor2, 1);
    PIO_Configure(&pinMotor3, 1);
    PIO_Configure(&pinMotor4, 1);
    
    // Enable PWMC peripheral clock
    AT91C_BASE_PMC->PMC_PCER |= 1 << AT91C_ID_PWMC;

    // Set clock A to run at 3276800Hz (clock B is not used)
    PWMC_ConfigureClocks(3276800, 0, BOARD_MCK);

    // Configure PWMC channel for LED0 (left-aligned)
    //PWMC_ConfigureChannel(CHANNEL_PWM_MOTOR1, AT91C_PWMC_CPRE_MCKA, 0, 0);
    PWMC_ConfigureChannel(CHANNEL_PWM_MOTOR2, AT91C_PWMC_CPRE_MCKA, 0, 0);
    PWMC_ConfigureChannel(CHANNEL_PWM_MOTOR3, AT91C_PWMC_CPRE_MCKA, 0, 0);
    PWMC_ConfigureChannel(CHANNEL_PWM_MOTOR4, AT91C_PWMC_CPRE_MCKA, 0, 0);
    
    //PWMC_SetPeriod(CHANNEL_PWM_MOTOR1, 50);
    //PWMC_SetDutyCycle(CHANNEL_PWM_MOTOR1, 10);
    
    PWMC_SetPeriod(CHANNEL_PWM_MOTOR2, 65535);
    PWMC_SetDutyCycle(CHANNEL_PWM_MOTOR2, 62258);
    
    PWMC_SetPeriod(CHANNEL_PWM_MOTOR3, 65535);
    PWMC_SetDutyCycle(CHANNEL_PWM_MOTOR3, 62258);
    
    PWMC_SetPeriod(CHANNEL_PWM_MOTOR4, 65535);
    PWMC_SetDutyCycle(CHANNEL_PWM_MOTOR4, 62258);
    
    TC_Start(AT91C_BASE_TC1);
    //PWMC_EnableChannel(CHANNEL_PWM_MOTOR1);
    PWMC_EnableChannel(CHANNEL_PWM_MOTOR2);
    PWMC_EnableChannel(CHANNEL_PWM_MOTOR3);
    PWMC_EnableChannel(CHANNEL_PWM_MOTOR4);
    
    // If they are present, configure Vbus & Wake-up pins
    //PIO_InitializeInterrupts(0);
    
    
    // Initialize flash driver
    //FLASHD_Initialize(BOARD_MCK);
    
    //pLastPageData = (unsigned int *) lastPageAddress;
    
    // Unlock whole flash
    //FLASHD_Unlock(AT91C_IFLASH, AT91C_IFLASH + AT91C_IFLASH_SIZE, 0, 0);
    
    //unsigned int var;
    //FLASHD_Write(lastPageAddress, &var, sizeof(var));
    //var = pLastPageData[i];
    
    
    // BOT driver initialization
    CDCDSerialDriver_Initialize();

    // connect if needed
    VBUS_CONFIGURE();
    
    Wait(200);
   
    SPI_Initialize();
    
    Wait(1);    
    
    //LED_Toggle(0);
    
    SPI_Read_Write(0x0303); //Communication register: write to Filter register; SF=03, f=1365.33 Hz
    
    //SPI_Write(AT91C_BASE_SPI, BOARD_ADC_NPCS, 0x020F); //Communication register: write to Control register; 1-st channel, unipolar, All Range
    //SPI_Read(AT91C_BASE_SPI);
    
    //SPI_Write(AT91C_BASE_SPI, BOARD_ADC_NPCS, 0x01D2); //Communication register: write to Mode register; no chop, 10 channel, single conversion
    //SPI_Read(AT91C_BASE_SPI);
    
    Wait(400);
    
    PIO_Set(&pinGyroReset);
    Delay(1000);
    PIO_Clear(&pinGyroReset);
    Wait(100);
    PIO_Set(&pinGyroReset);
    Delay(1000);
    PIO_Clear(&pinGyroReset);
    Wait(100);
    
    TC_Start(AT91C_BASE_TC0);    
    
    ADCConvert(ACCEL_X);
    
    //LED_Toggle(0);
    
    Wait(1000);
    
    accel_init();
    gyro_init();
    compass_init();
    
    pid_init(0, 1, 0, 0, 0, 0);
    pid_init(1, 1, 0, 0, 0, 0);
    pid_init(2, 1, 0, 0, 0, 0);
    
    ahrs_state = AHRS_IMU_CALIBRATION;
    //ahrs_state = AHRS_RUNNING;
    
    unsigned long Sum_GyroX = 0;
    unsigned long Sum_GyroY = 0;
    unsigned long Sum_GyroZ = 0;
    unsigned long TestCount = 15;
    unsigned long TestCurrent = 0;
    
    //ahrs_state = AHRS_RUNNING;
    // TODO: get data from IMU sensors and call roll_update(), pitch_update(), compass_update(), pqr_update()
    //_ahrs_init();

    // Driver loop
    while (1)
    {
      if (WorkState == STATE_RESUME)
      {
        if (ProgramState != STATE_CALCULATION) 
        { 
          if (!PIO_Get(&pinADCDataReady)) // ADC conversion complete
          {
            unsigned short Data = GetADCData();
            
            //unsigned short Header;
            
            switch (ProgramState)
            {
              case STATE_ACCEL_X:
              {
                ADCConvert(ACCEL_Y);
              
                accel_update_x(Data);
                
                break;
              
                //Header = 0x01FF;
              }
              case STATE_ACCEL_Y:
              {
                //Data = GetADCData();
                
                ADCConvert(ACCEL_Z);
                
                accel_update_y(Data);
                
                // TODO: calculate pitch after accelZ convertion
                
                
                
                break;
                
                //Header = 0x02FF;
              }
              case STATE_ACCEL_Z:
              {
                ADCConvert(GYRO_X);
                
                accel_update_z(Data);
                // Procces roll 
                if (ahrs_state != AHRS_IMU_CALIBRATION)
                {
                  roll_update(); // Y&Z 
                }
                
                if (ahrs_state != AHRS_IMU_CALIBRATION)
                {
                  pitch_update();//X,Y,Z
                }
                //if (ahrs_state == AHRS_RUNNING)
                //{
                  //ahrs_roll_update(ahrs_euler[0]);
                //}
                
                 // Process pitch
                
                //if (ahrs_state == AHRS_RUNNING)
                //{
                  // quaternion normalization here
                  //dcmupdate
                  //ahrs_pitch_update(ahrs_euler[1]);
                //}
                
                break;
                
                //Header = 0x03FF;
              }
              case STATE_GYRO_X:
              {
                ADCConvert(GYRO_Y);
                
                if (ahrs_state == AHRS_IMU_CALIBRATION)
                {
                  Sum_GyroX += Data;
                }
                
                if (ahrs_state != AHRS_IMU_CALIBRATION)
                {
                  gyro_update_x(Data);
                }
                
                break;
                
                //Header = 0x04FF;
              }
              case STATE_GYRO_Y:
              {
                ADCConvert(GYRO_Z);
                
                if (ahrs_state == AHRS_IMU_CALIBRATION)
                {
                  Sum_GyroY += Data;
                }
              
                if (ahrs_state != AHRS_IMU_CALIBRATION)
                {
                  gyro_update_y(Data);
                }
                
                break;
                
                //Header = 0x05FF;
              }
              case STATE_GYRO_Z:
              {
                ADCConvert(ACCEL_X);
                
                if (ahrs_state == AHRS_IMU_CALIBRATION)
                {
                  Sum_GyroZ += Data;
                }
                
                if (ahrs_state != AHRS_IMU_CALIBRATION)
                {
                  // Process gyro
                  gyro_update_z(Data);            
                
                  gyro_update();
                }
                
                break;
                //pqr_update();
                //if (ahrs_state == AHRS_RUNNING)
                //{
                  //ahrs_state_update();
                //}
                
                //Header = 0x06FF;
              }
              case STATE_MAG_X:
              {
                ADCConvert(MAG_Y);            
                
                compass_update_x(Data);
                
                break;
                
                //Header = 0x07FF;
              }
              case STATE_MAG_Y:
              {
                ADCConvert(MAG_Z);
                
                compass_update_y(Data);
                
                break;
                
                //Header = 0x08FF;
              }
              case STATE_MAG_Z:
              {
                ADCConvert(ACCEL_X);
                
                compass_update_z(Data);
                
                if (ahrs_state != AHRS_IMU_CALIBRATION)
                {
                  // Process compass
                  //compass_update();
                }
                
                break;
                
                //if (ahrs_state == AHRS_RUNNING)
                //{
                  //ahrs_compass_update(ahrs_euler[2]);
                //}
                
                //float a = 10.0;
              /*unsigned short Header = 0x00FF | (13 << 8);
              while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)&Header, sizeof(Header), 0, 0));
              while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)&quat[0], sizeof(float), 0, 0));
    
              Header = 0x00FF | (14 << 8);
              while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)&Header, sizeof(Header), 0, 0));
              while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)&quat[1], sizeof(float), 0, 0));
              
              Header = 0x00FF | (15 << 8);
              while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)&Header, sizeof(Header), 0, 0));
              while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)&quat[2], sizeof(float), 0, 0));
              
              Header = 0x00FF | (16 << 8);
              while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)&Header, sizeof(Header), 0, 0));
              while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)&quat[3], sizeof(float), 0, 0));*/
              
              //Header = 0x00FF | (17 << 8);
              //while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)&Header, sizeof(Header), 0, 0));
              //while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)&dcm00, sizeof(float), 0, 0));
                
                //Header = 0x09FF;
              }
            }
            WorkState = STATE_IDLE;
          }
          else
          {
            LED_Toggle(0);
          }
          
          //while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)&Header, sizeof(Header), 0, 0));
          //while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)&Data, sizeof(Data), 0, 0));
            
          
        }
        
        if (ProgramState == STATE_CALCULATION)
        {
          //LED_Toggle(0); 
          
          if (ahrs_state == AHRS_IMU_CALIBRATION)
          {
            TestCurrent++;
            if (TestCurrent > TestCount)
            {
              IMU_ADC_ROLL_DOT_ZERO = Sum_GyroX / (TestCount + 1);
              IMU_ADC_PITCH_DOT_ZERO = Sum_GyroY / (TestCount + 1);
              IMU_ADC_YAW_DOT_ZERO = Sum_GyroZ / (TestCount + 1);
              
              gyro_init();
              
              ahrs_state = AHRS_RUNNING;
            }
          }
          
          /*a = -1;
          if (ahrs_state == AHRS_NOT_INITIALIZED || a > 0)
          {
            a--;
            ahrs_init();
            
            ahrs_state = AHRS_RUNNING;
          }*/
          
          //PIO_Set(&pinGyroReset);
          //Wait(1);
          //PIO_Clear(&pinGyroReset);
            
          float PID[3];
          PID[0] = pid_update(0, ahrs_euler_real[0] - 0.0);
          PID[1] = pid_update(1, ahrs_euler_real[1] - 0.0);
          PID[2] = pid_update(2, ahrs_euler_real[2] - 0.0);
          
          if (Start == 1)
          {
            AT91C_BASE_TC1->TC_RA = (BOARD_MCK / divtc1) / ((1000) / (1.0 + Throttle));
            PWMC_SetDutyCycle(CHANNEL_PWM_MOTOR2, 62258 - (short)(Throttle * 3277.0));
            PWMC_SetDutyCycle(CHANNEL_PWM_MOTOR3, 62258 - (short)(Throttle * 3277.0));
            PWMC_SetDutyCycle(CHANNEL_PWM_MOTOR4, 62258 - (short)(Throttle * 3277.0));
          }
          else
          {
            AT91C_BASE_TC1->TC_RA = (BOARD_MCK / divtc1) / (1000);
            PWMC_SetDutyCycle(CHANNEL_PWM_MOTOR2, 62258);
            PWMC_SetDutyCycle(CHANNEL_PWM_MOTOR3, 62258);
            PWMC_SetDutyCycle(CHANNEL_PWM_MOTOR4, 62258);
          }
          
          PIO_Clear(&pinMagReset);
          Delay(400);
          PIO_Set(&pinMagReset);
          
          LED_Toggle(0);
          
          //quat2euler();
          
          if (ahrs_state != AHRS_IMU_CALIBRATION)
          {
            unsigned short Header;
            for (short i = 0; i < 3; i++)
            {
              //Header = 0x00FF | ((i+1) << 8);
              //while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)&Header, sizeof(Header), 0, 0));
              //while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)&accel[i], sizeof(accel[i]), 0, 0));
              
              //Header = 0x00FF | ((i+4) << 8);
              //while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)&Header, sizeof(Header), 0, 0));
              //while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)&pqr[i], sizeof(pqr[i]), 0, 0));
              
              //Header = 0x00FF | ((i+7) << 8);
              //while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)&Header, sizeof(Header), 0, 0));
              //while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)&mag[i], sizeof(mag[i]), 0, 0));
              
              Header = 0x00FF | ((i+10) << 8);
              while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)&Header, sizeof(Header), 0, 0));
              while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)&ahrs_euler_real[i], sizeof(ahrs_euler_real[i]), 0, 0));
              
              Header = 0x00FF | ((i+20) << 8);
              while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)&Header, sizeof(Header), 0, 0));
              while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)&ahrs_gyro[i], sizeof(ahrs_gyro[i]), 0, 0));
              
              Header = 0x00FF | ((i+23) << 8);
              while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)&Header, sizeof(Header), 0, 0));
              while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)&ahrs_accel[i], sizeof(ahrs_accel[i]), 0, 0));
              
              Header = 0x00FF | ((i+26) << 8);
              while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)&Header, sizeof(Header), 0, 0));
              while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)&PID[i], sizeof(PID[i]), 0, 0));
            }
          }
          
          
          
          //Header = 0x00FF | (17 << 8);
          //while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)&Header, sizeof(Header), 0, 0));
          //while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)&dcm22, sizeof(dcm22), 0, 0));
          
          WorkState = STATE_IDLE;
        }
      }
      
      // Read incoming messages from PC
      if (!usbReadRunning)
      {
        usbReadRunning = 1;
        CDCDSerialDriver_Read(usbBufferRead, DATABUFFERSIZE, (TransferCallback) UsbDataReceived, 0);        
      }
      
      Wait(1);
      
      if( USBState == USB_STATE_SUSPEND )
      {
          USBState = USB_STATE_IDLE;
      }
      if( USBState == USB_STATE_RESUME )
      {
          USBState = USB_STATE_IDLE;
      }
    }
}

//------------------------------------------------------------------------------
/// Unused code
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
/// SPI0 interrupt handler. Forwards the event to the SPI driver handler.
//------------------------------------------------------------------------------
/*void ISR_Spi0(void)
{
    //SDSPI_Handler(&sdSpiDrv);
}*/

/*void sendSPI(void)
{
   AT91C_BASE_SPI->SPI_RCR = 0x2;
   AT91C_BASE_SPI->SPI_TCR = 0x2;
   while(AT91C_BASE_SPI->SPI_RCR!=0);
}*/

/*spiBufferWrite = 0x0303; // Filter register: SF=03, f=1365.33 Hz
    SPI_WriteBuffer(AT91C_BASE_SPI, &spiBufferWrite, 2);
    //SPI_ReadBuffer(AT91C_BASE_SPI, &spiBufferRead, 2);
    
    spiBufferWrite = 0x020F; // Control register: 1-st channel, unipolar, All Range
    SPI_WriteBuffer(AT91C_BASE_SPI, &spiBufferWrite, 2);
    //SPI_ReadBuffer(AT91C_BASE_SPI, &spiBufferRead, 2);
    
    spiBufferWrite = 0x01D2; // Mode register: no chop, 10 channel, single conversion
    SPI_WriteBuffer(AT91C_BASE_SPI, &spiBufferWrite, 2);*/
    //SPI_ReadBuffer(AT91C_BASE_SPI, &spiBufferRead, 2);
    

///spiBufferWrite = 0x020F;
      ///SPI_WriteBuffer(AT91C_BASE_SPI, &spiBufferWrite, 2);
      //SPI_ReadBuffer(AT91C_BASE_SPI, &spiBufferRead, 2);
      /*SPI_Write(AT91C_BASE_SPI, 2, 0x02); //Communication register: write to Control register
      SPI_Read(AT91C_BASE_SPI);
      SPI_Write(AT91C_BASE_SPI, 2, 0x0F); //Control register: 1-st channel, unipolar, All Range
      SPI_Read(AT91C_BASE_SPI);*/
      
      //Wait(1000);
      //LED_Toggle(0);
      
      //spiBufferWrite = 0x44FF;
      //SPI_WriteBuffer(AT91C_BASE_SPI, &spiBufferWrite, 2);
      
      
      //LED_Toggle(0);
      
      /*spiBufferWrite = 0xFF44; // Read ADC Data register
      SPI_WriteBuffer(AT91C_BASE_SPI, &spiBufferWrite, 2);
      SPI_ReadBuffer(AT91C_BASE_SPI, &spiBufferRead, 2);
      
      spiBufferWrite = 0xFFFF;
      SPI_WriteBuffer(AT91C_BASE_SPI, &spiBufferWrite, 2);*/
      //SPI_ReadBuffer(AT91C_BASE_SPI, &spiBufferRead, 2);
      
      
      //spiBufferWrite = 0x01D2; // Mode register: no chop, 10 channel, single conversion
      //SPI_WriteBuffer(AT91C_BASE_SPI, &spiBufferWrite, 2);
      //SPI_ReadBuffer(AT91C_BASE_SPI, &spiBufferRead, 2);
      //unsigned short a = 5;
      
      //while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)&a, 2/*sizeof(spiBufferRead)*/, 0, 0));
      //while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)&b, 1/*sizeof(spiBufferRead)*/, 0, 0));
      //while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)&c, 1/*sizeof(spiBufferRead)*/, 0, 0));
      
      //Wait(100);
      //LED_Toggle(0);
      
      //spiBufferRead = 0;
      
      //SPI_ReadBuffer(AT91C_BASE_SPI, &spiBufferRead, 4);
      
      //while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)spiBufferRead, 4/*sizeof(spiBufferRead)*/, 0, 0));
      //SPI_ReadBuffer(AT91C_BASE_SPI, &spiBufferRead, 4);
      
      //while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)spiBufferRead, 4/*sizeof(spiBufferRead)*/, 0, 0));
    
      /*SPI_Write(AT91C_BASE_SPI, 2, 0x44); //Communication register: read from Data register
      unsigned short HData = SPI_Read(AT91C_BASE_SPI); //Data register high byte
      SPI_Write(AT91C_BASE_SPI, 2, 0x00);
      unsigned short LData = SPI_Read(AT91C_BASE_SPI); //Data register low byte*/
      //Wait(5);
      //LED_Toggle(0);
        // Device is not configured
        //if (USBD_GetState() < USBD_STATE_CONFIGURED) {

            // Connect pull-up, wait for configuration
            //USBD_Connect();
            //while (USBD_GetState() < USBD_STATE_CONFIGURED);

            // Start receiving data on the USART
            /*
            usartCurrentBuffer = 0;
            USART_ReadBuffer(AT91C_BASE_US0, usartBuffers[0], DATABUFFERSIZE);
            USART_ReadBuffer(AT91C_BASE_US0, usartBuffers[1], DATABUFFERSIZE);
            AT91C_BASE_US0->US_IER = AT91C_US_ENDRX
                                     | AT91C_US_FRAME
                                     | AT91C_US_OVER;
            AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;*/
      
            //int Zero  = 0;
      
            //while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)HData, sizeof(HData), (TransferCallback) UsbDataTransfered, 0));
            //while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)Zero, 1, (TransferCallback) UsbDataTransfered, 0));
            //while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)LData, sizeof(LData), (TransferCallback) UsbDataTransfered, 0));
            //while (USBD_STATUS_SUCCESS != CDCDSerialDriver_Write((void *)Zero, 1, (TransferCallback) UsbDataTransfered, 0));

            // Start receiving data on the USB
            //CDCDSerialDriver_Read(usbBufferRead, DATABUFFERSIZE, (TransferCallback) UsbDataReceived, 0);
       // }

//------------------------------------------------------------------------------
/// Handles interrupts coming from Timer #0.
//------------------------------------------------------------------------------
/*static void ISR_Timer0()
{
    unsigned char size;
    unsigned int status = AT91C_BASE_TC0->TC_SR;

    if ((status & AT91C_TC_CPCS) != 0) {
    
        // Flush PDC buffer
        size = DATABUFFERSIZE - AT91C_BASE_US0->US_RCR;
        if (size == 0) {

            AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
            return;
        }
        AT91C_BASE_US0->US_RCR = 0;
    
        // Send current buffer through the USB
        while (CDCDSerialDriver_Write(usartBuffers[usartCurrentBuffer],
                                      size, 0, 0) != USBD_STATUS_SUCCESS);
    
        // Restart read on buffer
        USART_ReadBuffer(AT91C_BASE_US0,
                         usartBuffers[usartCurrentBuffer],
                         DATABUFFERSIZE);
        usartCurrentBuffer = 1 - usartCurrentBuffer;
      LED_Toggle(0);
      
        AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
    }
}*/


